// 动作服务客户端节点
#include "base_interfase/action/nav.hpp"
#include "rclcpp_action/rclcpp_action.hpp" // 注意这个头文件
#include <memory>
#include <rclcpp/rclcpp.hpp>

using Nav = base_interfase::action::Nav;
using GoalHandleMove = rclcpp_action::ClientGoalHandle<Nav>;
using namespace std::placeholders;
class Exer04_action_client : public rclcpp::Node {

  rclcpp_action::Client<Nav>::SharedPtr action_client_;

public:
  Exer04_action_client() : Node("exer04_action_client") {
    RCLCPP_INFO(this->get_logger(), "初始化动作服务客户端");
    // 创建动作服务客户端
    action_client_ = rclcpp_action::create_client<Nav>(this, "nav");

    // // 定时器，等待动作服务服务器启动后发送目标
    // this->timer_ = this->create_wall_timer(
    //     std::chrono::milliseconds(500),
    //     std::bind(&Exer04_action_client::send_goal, this));
  }

  void send_goal(float x, float y, float theta) {
    using namespace std::placeholders;
    // this->timer_->cancel(); // 只执行一次

    // 等待动作服务服务器启动
    if (!this->action_client_->wait_for_action_server(
            std::chrono::seconds(10))) {
      RCLCPP_ERROR(this->get_logger(), "动作服务服务器未启动");
      rclcpp::shutdown();
      return;
    }

    auto goal_msg = Nav::Goal();
    goal_msg.x_goal = x;
    goal_msg.y_goal = y;
    goal_msg.theta_goal = theta;

    RCLCPP_INFO(this->get_logger(), "发送目标: x=%.2f, y=%.2f, theta=%.2f",
                goal_msg.x_goal, goal_msg.y_goal, goal_msg.theta_goal);

    auto send_goal_options = rclcpp_action::Client<Nav>::SendGoalOptions();
    send_goal_options.goal_response_callback =
        std::bind(&Exer04_action_client::goal_response_callback, this,
                  std::placeholders::_1);
    send_goal_options.feedback_callback =
        std::bind(&Exer04_action_client::feedback_callback, this, _1, _2);
    send_goal_options.result_callback =
        std::bind(&Exer04_action_client::result_callback, this, _1);

    action_client_->async_send_goal(goal_msg, send_goal_options);
  }

private:
  // rclcpp::TimerBase::SharedPtr timer_;

  // 目标响应回调
  void goal_response_callback(GoalHandleMove::SharedPtr goal_handle) {
    if (!goal_handle) {
      RCLCPP_ERROR(this->get_logger(), "目标被拒绝");
    } else {
      RCLCPP_INFO(this->get_logger(), "目标被接受");
    }
  }
  // 反馈回调
  void feedback_callback(GoalHandleMove::SharedPtr,
                         const std::shared_ptr<const Nav::Feedback> feedback) {
    RCLCPP_INFO(this->get_logger(), "当前距离目标还有: %.2f",
                feedback->distance);
  }
  // 结果回调
  void result_callback(const GoalHandleMove::WrappedResult &result) {
    switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "任务成功完成");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "任务被中止");
      return;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_ERROR(this->get_logger(), "任务被取消");
      return;
    default:
      RCLCPP_ERROR(this->get_logger(), "未知结果");
      return;
    }

    RCLCPP_INFO(this->get_logger(), "最终位置: x=%.2f, y=%.2f, theta=%.2f",
                result.result->x_turtle, result.result->y_turtle,
                result.result->theta_turtle);

    rclcpp::shutdown();
  }
};

int main(int argc, char **argv) {

  if (argc < 5) {
    RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "参数错误：%d", argc);
    return -1;
  }

  float x = atof(argv[1]);
  float y = atof(argv[2]);
  float thera = atof(argv[3]);

  rclcpp::init(argc, argv);
  auto action_client_node = std::make_shared<Exer04_action_client>();

  action_client_node->send_goal(x, y, thera);
  rclcpp::spin(action_client_node);
  rclcpp::shutdown();
  return 0;
}
